# coding:utf-8
from PyQt5 import QtWidgets
from PyQt5.QtCore import *
from PyQt5.QtWidgets import *
import pandas as pd
import numpy as np
import sys,threading,math
import cv2
import sim
import re
import random
import pyrealsense2 as rs
import model as md
import ControlBoard, detect_round
from dobot_api import DobotApiDashboard, DobotApi, DobotApiMove

class MyWindow(QtWidgets.QMainWindow, ControlBoard.Ui_mainWindow):
    def __init__(self):
        super(MyWindow, self).__init__()
        threading.Thread.__init__(self)
        self.setupUi(self)
        self.ComputeMatrixClass=ComputeMatrixClass()
        self.VrepClass = CoppeliasimClass()  # 类的实例化
        self.jointhandles = []
        self.targetpoint=[]
        self.trans_end2hand = [45.96, 45.96, 267, 0.0000001, 0.0000001, 0.0000001]#末端到手，可以量出
        self.MyTimer = QTimer(self)  # 实例化系统定时器
        # self.long_press_timer=QTimer(self)  # 创建一个长按检测定时器
        # self.long_press_timer.timeout.connect(self.longpressaction)
        self.clientID = -1  # 默认关闭Vrep
        self.Connect.clicked.connect(lambda :self.ConnectAll)
        self.Disconnect.clicked.connect(lambda: self.DisconnectAll)
        # 笛卡尔按键控制
        self.Xplus.clicked.connect(lambda : self.Cartmove(1,'+'))
        self.Xplus_3.clicked.connect(lambda: self.Cartmove(3,'+'))
        self.Xplus_5.clicked.connect(lambda: self.Cartmove(5,'+'))
        self.Xplus_7.clicked.connect(lambda: self.Cartmove(7,'+'))
        self.Xplus_9.clicked.connect(lambda: self.Cartmove(9,'+'))
        self.Xplus_11.clicked.connect(lambda: self.Cartmove(11,'+'))
        self.Xplus_2.clicked.connect(lambda: self.Cartmove(2, '-'))
        self.Xplus_4.clicked.connect(lambda: self.Cartmove(4, '-'))
        self.Xplus_6.clicked.connect(lambda: self.Cartmove(6, '-'))
        self.Xplus_8.clicked.connect(lambda: self.Cartmove(8, '-'))
        self.Xplus_10.clicked.connect(lambda: self.Cartmove(10, '-'))
        self.Xplus_12.clicked.connect(lambda: self.Cartmove(12, '-'))

        # 关节空间按键控制
        self.Jplus_7.clicked.connect(lambda: self.Joinmove(7, '+'))
        self.Jplus_9.clicked.connect(lambda: self.Joinmove(9, '+'))
        self.Jplus_11.clicked.connect(lambda: self.Joinmove(11, '+'))
        self.Jplus_13.clicked.connect(lambda: self.Joinmove(13, '+'))
        self.Jplus_15.clicked.connect(lambda: self.Joinmove(15, '+'))
        self.Jplus_17.clicked.connect(lambda: self.Joinmove(17, '+'))
        self.Jplus_8.clicked.connect(lambda: self.Joinmove(8, '-'))
        self.Jplus_10.clicked.connect(lambda: self.Joinmove(10, '-'))
        self.Jplus_12.clicked.connect(lambda: self.Joinmove(12, '-'))
        self.Jplus_14.clicked.connect(lambda: self.Joinmove(14, '-'))
        self.Jplus_16.clicked.connect(lambda: self.Joinmove(16, '-'))
        self.Jplus_18.clicked.connect(lambda: self.Joinmove(18, '-'))
        # self.Jplus_7.pressed.connect(self.startlongtimer)
        # self.Jplus_7.released.connect(self.stoplongtimer)
        # self.Jplus_9.pressed.connect(self.startlongtimer)
        # self.Jplus_9.released.connect(self.stoplongtimer)
        # self.Jplus_11.pressed.connect(self.startlongtimer)
        # self.Jplus_11.released.connect(self.stoplongtimer)
        # self.Jplus_13.pressed.connect(self.startlongtimer)
        # self.Jplus_13.released.connect(self.stoplongtimer)
        # self.Jplus_15.pressed.connect(self.startlongtimer)
        # self.Jplus_15.released.connect(self.stoplongtimer)
        # self.Jplus_17.pressed.connect(self.startlongtimer)
        # self.Jplus_17.released.connect(self.stoplongtimer)
        # self.Jplus_8.pressed.connect(self.startlongtimer)
        # self.Jplus_8.released.connect(self.stoplongtimer)
        # self.Jplus_10.pressed.connect(self.startlongtimer)
        # self.Jplus_10.released.connect(self.stoplongtimer)
        # self.Jplus_12.pressed.connect(self.startlongtimer)
        # self.Jplus_12.released.connect(self.stoplongtimer)
        # self.Jplus_14.pressed.connect(self.startlongtimer)
        # self.Jplus_14.released.connect(self.stoplongtimer)
        # self.Jplus_16.pressed.connect(self.startlongtimer)
        # self.Jplus_16.released.connect(self.stoplongtimer)
        # self.Jplus_18.pressed.connect(self.startlongtimer)
        # self.Jplus_18.released.connect(self.stoplongtimer)
        self.FileButton.clicked.connect(lambda: self.OpenFile())
        self.StartMove.clicked.connect(lambda: self.ThreadFile())
        # 运动控制模块
        self.enable.clicked.connect(lambda: self.Enable())
        self.disable.clicked.connect(lambda : self.Disable())
        self.circledetect.clicked.connect(lambda : self.Detectcircle())
        self.boltdetect.clicked.connect(lambda : self.Detectbolt())
        self.circledetect_2.clicked.connect(lambda : self.Moveto())
        self.fullmission.clicked.connect(lambda : self.Fulldisasmble())
        self.speed.clicked.connect(lambda: self.confirmspeed())
    # 执行主要操作
    def ConnectAll(self):
        try:
            sim.simxFinish(-1)
            self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)  # 定义ID的类内变量
            if self.clientID != -1:
                self.VrepLabel.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # Vrep指示灯亮
                self.LogText.append('Connect to Vrep')
                sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)  # 仿真初始化
                self.jointhandles = self.VrepClass.GetHandles(self.clientID, 6, 'UR5_joint')  # 得到六个关节角句柄
                self.ConnectRobot() # 初始化机器人相关控制代码
                if self.dashboard != False:
                    self.VrepLabel_2.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # robot指示灯亮
                    self.LogText.append('Connect to Robot')
                    self.dashboard.CP(99)
                    self.dashboard.SpeedFactor(10)  # 默认速度为10
                    # 相机连接函数
                    # self.ConnectCam()
                    self.pipe = rs.pipeline()
                    config = rs.config()
                    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
                    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
                    self.profile = self.pipe.start(config)
                    self.depth_intrinsics = self.profile.get_stream(
                        rs.stream.depth).as_video_stream_profile().get_intrinsics()
                    self.color_intrinsics = self.profile.get_stream(
                        rs.stream.color).as_video_stream_profile().get_intrinsics()
                    if self.profile:
                        self.VrepLabel_3.setStyleSheet('''QLabel{background-color: rgb(0, 234, 0)}''')  # camera指示灯亮
                        self.LogText.append('Connect to camera')
                        self.hand2cam = []  # 读取标定结果
                        # 打开并读取txt文件
                        with open('VisionModule/matlabprogram/end2cam5_8.txt', 'r') as cam2tool_file:
                            rota_data = cam2tool_file.readlines()
                        # 输出文件中的数据
                        for line in rota_data:
                            value = line.strip().split(',')  # 默认按空格分割
                            value = list(map(float, value))
                            value = np.array([num for num in value])
                            self.hand2cam.append(value)
                        self.hand2cam=np.array(self.hand2cam)
                        self.hand2cam[0:3,3]=self.hand2cam[0:3,3]*1000
                        cam2tool_file.close()
                        self.LogText.append('手眼矩阵加载完成')
                        self.MyTimer.timeout.connect(self.ReadTask)  # 定时执行函数   #####self.ReadTask
                        self.MyTimer.start(5)  # 定时时间5ms
                        # Skip 5 first frames to give the Auto-Exposure time to adjust
                        for x in range(10):
                            self.pipe.wait_for_frames()
                        while True:
                            frameset = self.pipe.wait_for_frames()
                            color_frame = frameset.get_color_frame()
                            # depth_frame = frameset.get_depth_frame()
                            color = np.asanyarray(color_frame.get_data())  # cv2.imread("D:/train_pictures/099_Color.png")#
                            # Create alignment primitive with color as its target stream:
                            cv2.imshow('RealSense', color)
                            key = cv2.waitKey(1)  # 等待按键输入，每毫秒检测一次
                            if key == ord('q') or key == ord('Q'):  # 如果按下 Q 键，则退出循环
                                cv2.destroyAllWindows()
                                break
                    else:
                        self.LogText.append(' Failed connect to camera')
                        self.LogText.append("请检查重试")
                else:
                    self.LogText.append(' Failed connect to Robot')
                    self.LogText.append("请检查重试")
            else:
                self.LogText.append("Failed connect to Vrep")
                self.LogText.append("请检查重试")
        except Exception as e:
            self.LogText.append(f'Connect failed {e}')
    # 断联
    def DisconnectAll(self):
        try:
            self.MyTimer.stop()  # 定时器停止
            sim.simxFinish(-1)
            self.VrepLabel.setStyleSheet('')  # 指示灯灭
            self.VrepLabel_2.setStyleSheet('')  # 指示灯灭
            self.VrepLabel_3.setStyleSheet('')  # 指示灯灭
            self.cam.release()
            self.pipe.stop()
            cv2.destroyAllWindows()
            self.LogText.append('断开所有连接')
        except Exception as e:
            self.LogText.append(f'断开失败 {e}')
    # 长按按键动作
    def longpressaction(self):
        j1 = float(self.J1text.toPlainText())
        j2 = float(self.J1text_2.toPlainText())
        j3 = float(self.J1text_3.toPlainText())
        j4 = float(self.J1text_4.toPlainText())
        j5 = float(self.J1text_5.toPlainText())
        j6 = float(self.J1text_6.toPlainText())
        movenum = 0
        if self.movenum == self.Jplus_7:
            j1 += 0.4
            self.move.MoveJog("J1+")
            self.LogText.append('关节空间运动J1+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_9:
            j2 += 0.4
            self.move.MoveJog("J2+")
            self.LogText.append('关节空间运动J2+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_11:
            j3 += 0.4
            self.move.MoveJog("J3+")
            self.LogText.append('关节空间运动J3+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_13:
            j4 += 0.4
            self.move.MoveJog("J4+")
            self.LogText.append('关节空间运动J4+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_15:
            j5 += 0.4
            self.move.MoveJog("J5+")
            self.LogText.append('关节空间运动J5+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_17:
            j6 += 0.4
            self.move.MoveJog("J6+")
            self.LogText.append('关节空间运动J6+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_8:
            j1 -= 0.4
            self.move.MoveJog("J1-")
            self.LogText.append('关节空间运动J1-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_10:
            j2 -= 0.4
            self.move.MoveJog("J2-")
            self.LogText.append('关节空间运动J2-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_12:
            j3 -= 0.4
            self.move.MoveJog("J3-")
            self.LogText.append('关节空间运动J3-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_14:
            j4 -= 0.4
            self.move.MoveJog("J4-")
            self.LogText.append('关节空间运动J4-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_16:
            j5 -= 0.4
            self.move.MoveJog("J5-")
            self.LogText.append('关节空间运动J5-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_18:
            j6 -= 0.4
            self.move.MoveJog("J6-")
            self.LogText.append('关节空间运动J6-')
            self.movenum = -1
            movenum = 2
        if movenum == 2:
            jpose = np.array([j1, j2, j3, j4, j5, j6])
            for i in range(6):
                sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], jpose[i], sim.simx_opmode_blocking)

        x = float(self.Xtext.toPlainText())
        y = float(self.Xtext_2.toPlainText())
        z = float(self.Xtext_3.toPlainText())
        rx = float(self.Xtext_4.toPlainText())
        ry = float(self.Xtext_5.toPlainText())
        rz = float(self.Xtext_6.toPlainText())
        if self.movenum == self.Xplus:
            x = x + 0.5
            self.move.MoveJog("X+")
            self.LogText.append('笛卡尔空间运动X+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_3:
            y = y + 0.5
            self.move.MoveJog("Y+")
            self.LogText.append('笛卡尔空间运动Y+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_5:
            z += 0.5
            self.move.MoveJog("Z+")
            self.LogText.append('笛卡尔空间运动Z+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_7:
            rx += 0.5
            self.move.MoveJog("Rx+")
            self.LogText.append('笛卡尔空间运动Rx+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_9:
            ry += 0.5
            self.move.MoveJog("Ry+")
            self.LogText.append('笛卡尔空间运动Ry+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_11:
            rz += 0.5
            self.move.MoveJog("Rz+")
            self.LogText.append('笛卡尔空间运动Rz+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_2:
            x = x - 0.5
            self.move.MoveJog("X-")
            self.LogText.append('笛卡尔空间运动X-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_4:
            y = y - 0.5
            self.move.MoveJog("Y-")
            self.LogText.append('笛卡尔空间运动Y-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_6:
            z -= 0.5
            self.move.MoveJog("Z-")
            self.LogText.append('笛卡尔空间运动Z-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_8:
            rx -= 0.5
            self.move.MoveJog("Rx-")
            self.LogText.append('笛卡尔空间运动Rx-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_10:
            ry -= 0.5
            self.move.MoveJog("Ry-")
            self.LogText.append('笛卡尔空间运动Ry-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_12:
            rz -= 0.5
            self.move.MoveJog("Rz-")
            self.LogText.append('笛卡尔空间运动Rz-')
            self.movenum = -1
            movenum = 1

        if movenum == 1:
            inverjoint = self.dashboard.InverseSolution(x, y, z, rx, ry, rz, 0, 0)
            # 使用正则表达式提取第一个大括号内的数据
            match = re.search(r'\{(.*?)\}', inverjoint)
            if match:
                joint = match.group(1)
                jpose = joint.strip().split(',')
                lastpose = np.array(list(map(float, jpose))) * np.pi / 180
                for i in range(6):
                    sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], lastpose[i],
                                                   sim.simx_opmode_blocking)
    # 笛卡尔空间运动，关节模式
    def Cartmove(self):
        x = float(self.Xtext.toPlainText())
        y = float(self.Xtext_2.toPlainText())
        z = float(self.Xtext_3.toPlainText())
        rx = float(self.Xtext_4.toPlainText())
        ry = float(self.Xtext_5.toPlainText())
        rz = float(self.Xtext_6.toPlainText())
        movenum=0
        if self.movenum == self.Xplus:
            x = x + 0.5
            self.move.MoveJog("X+")
            self.LogText.append('笛卡尔空间运动X+')
            self.movenum=-1
            movenum = 1
        elif self.movenum == self.Xplus_3:
            y = y + 0.5
            self.move.MoveJog("Y+")
            self.LogText.append('笛卡尔空间运动Y+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_5:
            z += 0.5
            self.move.MoveJog("Z+")
            self.LogText.append('笛卡尔空间运动Z+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_7:
            rx += 0.5
            self.move.MoveJog("Rx+")
            self.LogText.append('笛卡尔空间运动Rx+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_9:
            ry += 0.5
            self.move.MoveJog("Ry+")
            self.LogText.append('笛卡尔空间运动Ry+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_11:
            rz += 0.5
            self.move.MoveJog("Rz+")
            self.LogText.append('笛卡尔空间运动Rz+')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_2:
            x = x - 0.5
            self.move.MoveJog("X-")
            self.LogText.append('笛卡尔空间运动X-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_4:
            y = y - 0.5
            self.move.MoveJog("Y-")
            self.LogText.append('笛卡尔空间运动Y-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_6:
            z -= 0.5
            self.move.MoveJog("Z-")
            self.LogText.append('笛卡尔空间运动Z-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_8:
            rx -= 0.5
            self.move.MoveJog("Rx-")
            self.LogText.append('笛卡尔空间运动Rx-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_10:
            ry -= 0.5
            self.move.MoveJog("Ry-")
            self.LogText.append('笛卡尔空间运动Ry-')
            self.movenum = -1
            movenum = 1
        elif self.movenum == self.Xplus_12:
            rz -= 0.5
            self.move.MoveJog("Rz-")
            self.LogText.append('笛卡尔空间运动Rz-')
            self.movenum = -1
            movenum = 1

        if movenum==1:
            inverjoint = self.dashboard.InverseSolution(x, y, z, rx, ry, rz, 0, 0)
            # 使用正则表达式提取第一个大括号内的数据
            match = re.search(r'\{(.*?)\}', inverjoint)
            if match:
                joint = match.group(1)
                jpose = joint.strip().split(',')
                lastpose = np.array(list(map(float, jpose))) * np.pi / 180
                for i in range(6):
                    sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], lastpose[i],
                                                   sim.simx_opmode_blocking)
    # 关节空间运动
    def Joinmove(self):
        j1=float(self.J1text.toPlainText())
        j2 = float(self.J1text_2.toPlainText())
        j3 = float(self.J1text_3.toPlainText())
        j4 = float(self.J1text_4.toPlainText())
        j5 = float(self.J1text_5.toPlainText())
        j6 = float(self.J1text_6.toPlainText())
        movenum=0
        if self.movenum == self.Jplus_7:
            j1 += 0.4
            self.move.MoveJog("J1+")
            self.LogText.append('关节空间运动J1+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_9:
            j2 += 0.4
            self.move.MoveJog("J2+")
            self.LogText.append('关节空间运动J2+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_11:
            j3 += 0.4
            self.move.MoveJog("J3+")
            self.LogText.append('关节空间运动J3+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_13:
            j4 += 0.4
            self.move.MoveJog("J4+")
            self.LogText.append('关节空间运动J4+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_15:
            j5 += 0.4
            self.move.MoveJog("J5+")
            self.LogText.append('关节空间运动J5+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_17:
            j6 += 0.4
            self.move.MoveJog("J6+")
            self.LogText.append('关节空间运动J6+')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_8:
            j1 -= 0.4
            self.move.MoveJog("J1-")
            self.LogText.append('关节空间运动J1-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_10:
            j2 -= 0.4
            self.move.MoveJog("J2-")
            self.LogText.append('关节空间运动J2-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_12:
            j3 -= 0.4
            self.move.MoveJog("J3-")
            self.LogText.append('关节空间运动J3-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_14:
            j4 -= 0.4
            self.move.MoveJog("J4-")
            self.LogText.append('关节空间运动J4-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_16:
            j5 -= 0.4
            self.move.MoveJog("J5-")
            self.LogText.append('关节空间运动J5-')
            self.movenum = -1
            movenum = 2
        elif self.movenum == self.Jplus_18:
            j6 -= 0.4
            self.move.MoveJog("J6-")
            self.LogText.append('关节空间运动J6-')
            self.movenum = -1
            movenum = 2
        if movenum == 2:
            jpose = np.array([j1, j2, j3, j4, j5, j6])
            for i in range(6):
                sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], jpose[i], sim.simx_opmode_blocking)

    # 打开路径文件
    def OpenFile(self):
        filename, _ = QFileDialog.getOpenFileName(self, '请选择文件', '', 'CSV(*.csv)')
        self.LogText.append(f'添加路径文件{filename}')
        self.interpolationpath = pd.read_csv(filename)  # 路径变量，便于全局使用
        self.interpolationpath = self.interpolationpath.to_numpy()
    # 开启另一个线程解决耗时任务
    def ThreadFile(self):
        task1 = threading.Thread(target=self.StartPath)  # 这里的StartPath不能加括号，要不开启不了线程
        task1.setDaemon(True)
        task1.start()
    # 路径开始，笛卡尔空间，关节模式
    def StartPath(self):
        for point in self.interpolationpath:
            self.move.MovJ(point[0],point[1],point[2],point[3],point[4],point[5],0,0)
            self.LogText.append(f'笛卡尔空间中关节运动:{str(point)}')
            # 提取角度，传给仿真平台
            angle=self.dashboard.GetAngle()
            # 使用正则表达式提取第一个大括号内的数据
            match = re.search(r'\{(.*?)\}', angle)
            if match:
                zhengjie = match.group(1)
                jpose = zhengjie.strip().split(',')
                rota = np.array(list(map(float, jpose))) * np.pi / 180
            for i in range(6):
                sim.simxSetJointTargetPosition(self.clientID, self.jointhandles[i], rota[i], sim.simx_opmode_blocking)
    # 机器人使能
    def Enable(self):
        print("开始使能...")
        try:
            self.dashboard.EnableRobot()  # enablerobot
        except Exception as e:
            print("使能失败")
            sys.exit(0)
        print("使能成功:)")
        self.LogText.append('机器人使能成功')
    # 机器人失能
    def Disable(self):
        try:
            self.dashboard.DisableRobot()
        except Exception as e:
            sys.exit(0)
        self.LogText.append('机器人下使能成功')
    # 识别孔洞
    def Detectcircle(self):
        while True:
            frameset = self.pipe.wait_for_frames()
            color_frame = frameset.get_color_frame()
            color = np.asanyarray(color_frame.get_data())  # cv2.imread("D:/train_pictures/099_Color.png")#
            # Create alignment primitive with color as its target stream:
            align = rs.align(rs.stream.color)
            frameset = align.process(frameset)
            # Update color and depth frames:
            aligned_depth_frame = frameset.get_depth_frame()
            gaussian_blur = cv2.GaussianBlur(color, (9, 9), 0)  # 第二个参数是滤波器的大小，第三个参数是标准差
            sharpening_kernel = np.array([[-1, -1, -1],
                                          [-1, 10, -1],
                                          [-1, -1, -1]])
            img0 = cv2.filter2D(gaussian_blur, -1, sharpening_kernel)
            i = 0
            round = detect_round.Detect_shapes_2(img0, 0)  # ,draw_center
            [ellipse_center, ellipse_axes, ellipse_angle] = round[0]
            while True:
                # 随机取点，沿椭圆中心到椭圆边缘构造一个向量
                i += 1
                angle = np.random.uniform(0, 2 * np.pi)
                vec = (ellipse_axes[0] * np.cos(angle) / 2, ellipse_axes[1] * np.sin(angle) / 2)

                # 将向量延长到原来的1.03倍并取整
                extended_point = [int(ellipse_center[0] + vec[0] * 1.03), int(ellipse_center[1] + vec[1] * 1.03)]

                # 与该点关于中心对称的另一点并取整
                symmetric_point = [int(2 * ellipse_center[0] - extended_point[0]),
                                   int(2 * ellipse_center[1] - extended_point[1])]

                # 检测两个点的深度
                depth_1 = aligned_depth_frame.get_distance(extended_point[0], extended_point[1])
                depth_2 = aligned_depth_frame.get_distance(symmetric_point[0], symmetric_point[1])

                # 如果两个点的深度均不为零，则跳出循环
                if depth_1 != 0 and depth_2 != 0:
                    break
                if i > 100:
                    print("it's too close to get distance information")
                    break
            # 椭圆信息
            depth_point_1 = np.array(
                rs.rs2_deproject_pixel_to_point(self.color_intrinsics, [extended_point[0], extended_point[1]], depth_1))
            depth_point_2 = np.array(
                rs.rs2_deproject_pixel_to_point(self.color_intrinsics, [symmetric_point[0], symmetric_point[1]],
                                                depth_2))
            #------得到检测的圆的中心坐标，相对于相机的
            center_pose = (depth_point_1 + depth_point_2) / 2

            cv2.putText(img0, str(center_pose), [int(ellipse_center[0]), int(ellipse_center[1])],
                        cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 2)
            cv2.imshow("Circle_detect", img0)
            key = cv2.waitKey(0)
            if key == ord('n') or key == ord('N'):
                continue
            if key == ord('y') or key == ord('Y'):
                cv2.destroyWindow("Circle_detect")
                break
        # ------得到检测的圆的中心坐标，相对于相机的,赋值为类变量
        center_pose=np.array([center_pose])*1000
        self.targetpoint=center_pose
        self.LogText.append('孔洞检测已完成：')
        self.LogText.append(str(self.targetpoint))
    """    
    # 识别孔洞
    def Detectcircle(self):
        while True:
            frameset = self.pipe.wait_for_frames()
            color_frame = frameset.get_color_frame()
            color = np.asanyarray(color_frame.get_data())  # cv2.imread("D:/train_pictures/099_Color.png")#
            # Create alignment primitive with color as its target stream:
            align = rs.align(rs.stream.color)
            frameset = align.process(frameset)
            # Update color and depth frames:
            aligned_depth_frame = frameset.get_depth_frame()
            gaussian_blur = cv2.GaussianBlur(color, (9, 9), 0)  # 第二个参数是滤波器的大小，第三个参数是标准差
            sharpening_kernel = np.array([[-1, -1, -1],
                                          [-1, 10, -1],
                                          [-1, -1, -1]])
            img0 = cv2.filter2D(gaussian_blur, -1, sharpening_kernel)
            i = 0
            round = detect_round.Detect_shapes_2(img0, 0)  # ,draw_center
            [ellipse_center, ellipse_axes, ellipse_angle] = round[0]
            while True:
                # 随机取点，沿椭圆中心到椭圆边缘构造一个向量
                i += 1
                angle = np.random.uniform(0, 2 * np.pi)
                vec = (ellipse_axes[0] * np.cos(angle) / 2, ellipse_axes[1] * np.sin(angle) / 2)

                # 将向量延长到原来的1.03倍并取整
                extended_point = [int(ellipse_center[0] + vec[0] * 1.03), int(ellipse_center[1] + vec[1] * 1.03)]

                # 与该点关于中心对称的另一点并取整
                symmetric_point = [int(2 * ellipse_center[0] - extended_point[0]),
                                   int(2 * ellipse_center[1] - extended_point[1])]

                # 检测两个点的深度
                depth_1 = aligned_depth_frame.get_distance(extended_point[0], extended_point[1])
                depth_2 = aligned_depth_frame.get_distance(symmetric_point[0], symmetric_point[1])

                # 如果两个点的深度均不为零，则跳出循环
                if depth_1 != 0 and depth_2 != 0:
                    break
                if i > 100:
                    print("it's too close to get distance information")
                    break
            # 椭圆信息
            depth_point_1 = np.array(
                rs.rs2_deproject_pixel_to_point(self.color_intrinsics, [extended_point[0], extended_point[1]], depth_1))
            depth_point_2 = np.array(
                rs.rs2_deproject_pixel_to_point(self.color_intrinsics, [symmetric_point[0], symmetric_point[1]],
                                                depth_2))
            #------得到检测的圆的中心坐标，相对于相机的
            center_pose = (depth_point_1 + depth_point_2) / 2

            cv2.putText(img0, str(center_pose), [int(ellipse_center[0]), int(ellipse_center[1])],
                        cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 2)
            cv2.imshow("Circle_detect", img0)
            key = cv2.waitKey(0)
            if key == ord('n') or key == ord('N'):
                continue
            if key == ord('y') or key == ord('Y'):
                cv2.destroyWindow("Circle_detect")
                break
        # ------得到检测的圆的中心坐标，相对于相机的,赋值为类变量
        # robotpose = self.dashboard.GetPose()  # 获取末端到基坐标系的变换矩阵
        # # 使用正则表达式提取第一个大括号内的数据
        # match = re.search(r'\{(.*?)\}', robotpose)
        # if match:
        #     robot = match.group(1)
        #     jpose = robot.strip().split(',')
        #     end2base = np.array([list(map(float, jpose))])
        center_pose=np.array([center_pose])*1000
        self.targetpoint=center_pose
        self.LogText.append('孔洞检测已完成：')
        self.LogText.append(str(self.targetpoint))
        """
    # 识别螺栓
    def Detectbolt(self):
        while True:
            frameset = self.pipe.wait_for_frames()
            color_frame = frameset.get_color_frame()
            color = np.asanyarray(color_frame.get_data())
            # Create alignment primitive with color as its target stream:
            align = rs.align(rs.stream.color)
            frameset = align.process(frameset)
            # Update color and depth frames:
            aligned_depth_frame = frameset.get_depth_frame()
            # 定义锐化核
            gaussian_blur = cv2.GaussianBlur(color, (5, 5), 0)  # 第二个参数是滤波器的大小，第三个参数是标准差
            sharpening_kernel = np.array([[-1, -1, -1],
                                          [-1, 10, -1],
                                         [-1, -1, -1]])
            # 应用锐化核
            imag0 = cv2.filter2D(gaussian_blur, -1, sharpening_kernel)
            # imag0 = color
            ret_img, bolt_point = md.get_bolts_pixel(imag0)
            #-----获得螺栓中心相对于相机的中心坐标并存储
            pose_bolt_list = []
            for point in bolt_point:
                depth_bolt = aligned_depth_frame.get_distance(point[0], point[1])
                if depth_bolt == 0:
                    i = 0
                    while True:
                        randomx = random.randint(-5, 5)
                        randomy = random.randint(-5, 5)
                        depth_bolt = aligned_depth_frame.get_distance(point[0] + randomx, point[1] + randomy)
                        if i >= 200 or depth_bolt != 0:
                            break
                        i = i + 1
                pose_bolt = np.array(
                    rs.rs2_deproject_pixel_to_point(self.color_intrinsics, [point[0], point[1]], depth_bolt))
                round_pose_bolt = [np.round(num, 3) for num in pose_bolt]
                pose_bolt_list.append(round_pose_bolt)
                cv2.putText(imag0, str(round_pose_bolt), point, cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 2)
                cv2.imshow("Bolt_detect", imag0)
            # 输出结果
            key = cv2.waitKey(0)
            if key == ord('n') or key == ord('N'):
                continue
            elif key == ord('y') or key == ord('Y'):
                cv2.destroyWindow("Bolt_detect")
                break
        # -----获得螺栓中心相对于相机的中心坐标并存储,转变为类变量
        pose_bolt_list=np.array(pose_bolt_list)*1000 # 转为mm单位
        self.targetpoint=pose_bolt_list
        self.LogText.append('螺栓检测已完成：')
        self.LogText.append(str(self.targetpoint))
    #  运动至
    def Moveto(self):
        position_in_robot=self.cam2base(point=self.targetpoint)
        self.dashboard.SpeedFactor(10)
        self.LogText.append('运动速率为：10')
        posi_in_robot=np.array([position_in_robot])
        print('postion',posi_in_robot)
        self.LogText.append(str(posi_in_robot))
        self.move.MoveJ(posi_in_robot[0,0],posi_in_robot[0,1],posi_in_robot[0,2],posi_in_robot[0,3],posi_in_robot[0,4],
                          posi_in_robot[0,5],0,0)
        self.LogText.append(f'笛卡尔空间关节运动：{str(posi_in_robot)}')
    # 完成拆卸任务
    def Fulldisasmble(self):
        self.LogText.append('开启完整拆卸任务')
        self.Detectcircle()
        self.Moveto()
        self.Detectbolt()
        self.Moveto()
    # 速度确认
    def confirmspeed(self):
        self.fullspeed = int(self.speedtext.toPlainText())
        self.dashboard.SpeedFactor(self.fullspeed)
        self.LogText.append(f'全局速度因子为：{str(self.fullspeed)}')
    # 读取数据
    def ReadTask(self):
        pose=self.dashboard.GetPose()
        # 使用正则表达式提取第一个大括号内的数据
        cmatch = re.search(r'\{(.*?)\}', pose)
        if cmatch:
            zhengjie = cmatch.group(1)
            cpose = zhengjie.strip().split(',')
            # cpose=[format(num, '.3f') for num in cpose]
            self.Xtext.setText(cpose[0])
            self.Xtext_2.setText(cpose[1])
            self.Xtext_3.setText(cpose[2])
            self.Xtext_4.setText(cpose[3])
            self.Xtext_5.setText(cpose[4])
            self.Xtext_6.setText(cpose[5])
        angle=self.dashboard.GetAngle()
        # 使用正则表达式提取第一个大括号内的数据
        jmatch = re.search(r'\{(.*?)\}', angle)
        if jmatch:
            zheng = jmatch.group(1)
            jpose = zheng.strip().split(',')
            self.J1text.setText(jpose[0])
            self.J1text_2.setText(jpose[1])
            self.J1text_3.setText(jpose[2])
            self.J1text_4.setText(jpose[3])
            self.J1text_5.setText(jpose[4])
            self.J1text_6.setText(jpose[5])
    # 将相机获取到的空间点坐标转换到机器人基座标下
    def cam2base(self,point):
        point=point[0,:]
        point=np.hstack((point,[0,0,0]))
        print('物体在相机下位姿:', point)
        robotpose=self.dashboard.GetPose() # 获取末端到基坐标系的变换矩阵
        # 使用正则表达式提取第一个大括号内的数据
        match = re.search(r'\{(.*?)\}', robotpose)
        if match:
            robot = match.group(1)
            jpose = robot.strip().split(',')
            end2base = np.array(list(map(float, jpose)))
            # end2base= self.ComputeMatrixClass.pose_to_matrix(end2base)
            print('end2base:', end2base)
        hand2basemt = self.ComputeMatrixClass.multiply_transforms(end2base, self.trans_end2hand) #手在基座标的位姿
        hand2base = self.ComputeMatrixClass.matrix_to_pose(hand2basemt)
        print('hand2base:', hand2base)
        cameramt=self.ComputeMatrixClass.multiply_transforms(hand2base, self.hand2cam) # 相机在基座标的位姿
        camerapose=self.ComputeMatrixClass.matrix_to_pose(cameramt)
        print('camera2base:',camerapose)
        point2basemt = self.ComputeMatrixClass.multiply_transforms(camerapose, point)# 螺栓在基座标的位姿
        point = self.ComputeMatrixClass.matrix_to_pose(point2basemt)
        print('Bolt2base:',  point)
        point = point - hand2base + end2base
        return point
    # tcp指令返回值处理函数，返回值为字符串里包含数字的数组
    def parseResultId(valueRecv):
        if valueRecv.find("Not Tcp") != -1:  # 通过返回值判断机器是否处于tcp模式
            print("Control Mode Is Not Tcp")
            return [-2]
        commandArrID = re.findall(r'-?\d+', valueRecv)
        # print("commandArrID", commandArrID)
        commandArrID = [int(num) for num in commandArrID]
        if commandArrID[0] == 0:
            if len(commandArrID) >= 2:
                return commandArrID  # 获取运动指令的commandArrID
            return [-1]
        else:
            # 根据返回值来判断机器处于什么状态
            if commandArrID[0] == -1:
                print("Queue command exceeds queue depth 64")
            elif commandArrID[0] == -2:
                print("The robot is in an error state")
            elif commandArrID[0] == -3:
                print("The robot is in emergency stop state")
            elif commandArrID[0] == -4:
                print("The robot is in power down state")
            else:
                print("The robot is in Uknown state", commandArrID[0])
        return [-2]
    # 机器tcp连接函数
    def ConnectRobot(self):
        try:
            ip = "192.168.5.1"
            dashboardPort = 29999
            movePort = 30003
            feedPort = 30004
            print("正在建立连接...")
            self.dashboard = DobotApiDashboard(ip, dashboardPort)
            self.move = DobotApiMove(ip, movePort)
            self.feed = DobotApi(ip, feedPort)
            print(">.<连接成功>!<")
            self.LogText.append(f'连接到{ip}')
        except Exception as e:
            print(":(连接失败:(")
            raise e
    # 连接相机
    def ConnectCam(self):
        task2 = threading.Thread(target=self.Camerathread)  # 这里的StartPath不能加括号，要不开启不了线程
        task2.setDaemon(True)
        task2.start()
    # 开启线程连接相机
    def Camerathread(self):
        self.pipe = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
        config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
        self.profile = self.pipe.start(config)
        self.depth_intrinsics = self.profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics()
        self.color_intrinsics = self.profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()

    # 长按定时器开启与关闭
    def startlongtimer(self):
        self.movenum=self.sender()
        self.long_press_timer.start(10)  # 设置定时器间隔为5ms秒
    def stoplongtimer(self):
        self.movenum=-1
        self.long_press_timer.stop()

class CoppeliasimClass():
    def GetHandles(self, client, number, name):
        objecthandle = []
        if number < 2:
            _, objecthandle = sim.simxGetObjectHandle(client, name, sim.simx_opmode_blocking)
            return objecthandle
        else:
            for i in range(number):
                _, Handle = sim.simxGetObjectHandle(client, name + str(i + 1), sim.simx_opmode_blocking)
                objecthandle.append(Handle)
            return objecthandle

class ComputeMatrixClass():
    # 欧拉角转为矩阵
    def angle2rotation(self,x, y, z):
        Rx = np.array([[1, 0, 0], [0, math.cos(x), -math.sin(x)], [0, math.sin(x), math.cos(x)]])
        Ry = np.array([[math.cos(y), 0, math.sin(y)], [0, 1, 0], [-math.sin(y), 0, math.cos(y)]])
        Rz = np.array([[math.cos(z), -math.sin(z), 0], [math.sin(z), math.cos(z), 0], [0, 0, 1]])
        R = Rz @ Ry @ Rx
        return R

    # 矩阵转为欧拉角
    def rotation2euler_angles(self,R):
        sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
        singular = sy < 1e-6
        if not singular:
            x = math.atan2(R[2, 1], R[2, 2])
            y = math.atan2(-R[2, 0], sy)
            z = math.atan2(R[1, 0], R[0, 0])
        else:
            x = math.atan2(-R[1, 2], R[1, 1])
            y = math.atan2(-R[2, 0], sy)
            z = 0
        return np.array([x, y, z])
    def pose_to_matrix(self,pose): # 传进来的是数组才行
        """
        Convert pose information [x, y, z, rx, ry, rz] to a 4x4 transformation matrix.
        """
        pose=np.array([pose])
        x, y, z, rx, ry, rz = pose[0,0],pose[0,1],pose[0,2],pose[0,3],pose[0,4],pose[0,5]
        translation = np.array([[x], [y], [z]])
        rotation = self.angle2rotation(rx, ry, rz)[0]
        matrix = np.vstack((np.hstack((rotation, translation)), [0, 0, 0, 1]))
        return matrix
    def matrix_to_pose(self,matrix):
        """
        Convert a 4x4 transformation matrix to pose information [x, y, z, rx, ry, rz].
        """
        translation = matrix[:3, 3]
        rotation = matrix[:3, :3]
        R_vect=self.rotation2euler_angles(rotation)
        pose = np.array([translation[0], translation[1], translation[2], R_vect[0,0],R_vect[1,0],R_vect[2,0]])
        return pose
    def multiply_transforms(self,ini_pose, trans_pose): # 传进来一个向量和一个矩阵
        """
        Multiply two transformation matrices obtained from initial and transformation poses.
        """
        T_ini = self.pose_to_matrix(ini_pose)
        T_trans = self.pose_to_matrix(trans_pose)
        T_final = np.dot(T_ini, T_trans)
        return T_final


if __name__=='__main__':
    app = QtWidgets.QApplication(sys.argv)
    window = MyWindow()
    window.show()
    sys.exit(app.exec_())